﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Server.Core.Filter
{
    public class KalmanFilter : IFilter
    {
        private double _A, _H, _P, _Q, _R;
        private double _K, _z, _x = 0;
        //private List<double> _data = new List<double>();

        public KalmanFilter(double A, double H, double P, double Q, double R)
        {
            _A = A;
            _H = H;
            _P = P;
            _Q = Q;
            _R = R;
        }

        public double processData(double input)
        {
            if (_x == 0)
            {
                _x = input;
            }
            // get current measured value
            _z = input;
            // time update - prediction
            _x *= _A;
            _P = _A * _P * _A + _Q;

            // measurement update - correction
            _K = _P * _H / (_H * _P * _H + _R);
            _P = (1 - _K * _H) * _P;
            _x = _x + _K * (_z - _H * _x);

            return _x;
        }
    }
}
